#ifndef ESTOP_H_
#define ESTOP_H_

#include "Command.h"
#include "Protocol.h"
#include "Reset.h"
#include "Periodic.h"

class CommandEStop : public Command
{
public:
	CommandEStop() : Command()
	{
		MyCommandType = EStop;
		ArgumentsLength = 0;
	}

	FailCode DoCommand()
	{
		// No interrupts shall stop the EStop.
		noInterrupts();

		// Stop every motor!  Stop every output!  Save the children!
		for (int i = 0; i < 14; i++)
			digitalWrite(i, LOW);

		// Send success.
		SendResponse(Success, MyCommandType);

		// Send EStop notification.
		SendNotification(NotificationEStop);

		SetEStopped(true);

		// Enable interrupts so the timer handler can work.
		interrupts();

		// Stop the code by looping for an eternity.
		while (true)
		{
			// Stop every motor!  Stop every output!  Save the children!
			for (int i = 0; i < 14; i++)
				digitalWrite(i, LOW);
		}

		// Return a fail code.  I guess our eternity went AWOL.
		return UnknownFail;
	}
};

#endif
